#include "StompedReality/Math/Matrix.h"
#include "StompedReality/Math/Vector2.h"
#include <algorithm>
#include <iostream>
#include <math.h>
#include <sstream>
namespace StompedReality
{
namespace Math
{
Matrix::Matrix( )
{
  this->m_Matrix  = ( float* ) malloc ( sizeof ( float ) * 16 );
  for ( int i = 0; i < 16; i++ )
  {
    this->m_Matrix[i] = 0;
  }
}

Matrix::~Matrix( )
{
  if ( this->m_Matrix != NULL )
  {
    free ( this->m_Matrix );
    this->m_Matrix = NULL;
  }
}

Matrix* Matrix::CreateDM( )
{
  Matrix* r = new Matrix( );
  r->m_Matrix[0] = 1;
  r->m_Matrix[5] = 1;
  r->m_Matrix[10] = 1;
  r->m_Matrix[15] = 1;
  return r;
}

void Matrix::Translate ( float x, float y, float d )
{
  this->m_Matrix[3] += x;
  this->m_Matrix[7] += y;
  this->m_Matrix[11] += d;
}

void Matrix::Translate ( StompedReality::Math::Vector2* tr )
{
  this->m_Matrix[3] += tr->GetX( );
  this->m_Matrix[7] += tr->GetY( );
}

StompedReality::Math::Vector2* Matrix::getTranslation( )
{
  return new StompedReality::Math::Vector2 ( this->m_Matrix[3], this->m_Matrix[7] );
}

Matrix* Matrix::CreateTranslationMatrix ( float x, float y, float d )
{
  Matrix* m = CreateDM( );
  m->Translate ( x, y, d );
  return m;
}

Matrix* Matrix::CreateTranslationMatrix ( StompedReality::Math::Vector2* v )
{
  Matrix* m = CreateDM( );
  m->Translate ( v->GetX( ), v->GetY( ) );
  return m;
}

Matrix* Matrix::Multiply ( Matrix* other )
{
  Matrix* r = new Matrix( );
  for ( int i = 0; i < 16; i++ )
  {
    int a = i % 4;
    int b = i / 4;
    for ( int k = 0; k < 4; k++ )
    {
      r->m_Matrix[i] += this->m_Matrix[k + b * 4] * other->m_Matrix[a + k * 4];
    }
  }
  std::copy ( r->m_Matrix, r->m_Matrix + 16, this->m_Matrix );
  delete r;
  return this;
}

Matrix* Matrix::CreateRotationMatrix ( float alpha )
{
  Matrix* m = CreateDM( );
  m->m_Matrix[0] = cos ( alpha );
  m->m_Matrix[1] = -sin ( alpha );
  m->m_Matrix[4] = sin ( alpha );
  m->m_Matrix[5] = cos ( alpha );
  return m;
}

void Matrix::Print( )
{
  std::stringstream s;
  for ( int i = 0; i < 4; i++ )
  {
    for ( int j = 0; j < 4; j++ )
    {
      s << this->m_Matrix[i * 4 + j] << " ";
    }
    s << std::endl;
  }
  std::cout << s.str( ) << std::endl;
}

}
}
